fix(planning,control): stabilize reverse behavior and decouple front-distance reflex by xiaolefang-dm · Pull Request #124 · UniflexAI/tinynav

xiaolefang-dm · 2026-04-29T08:23:15Z

Move reverse selection into planning_node: regular trajectory sampling is now forward-only, with one predefined straight reverse candidate.
Gate reverse behavior using planner-side front-clearance detection instead of coupling front-obstacle reflex logic into cmd_vel_control.
Update trajectory generation from acceleration sampling to velocity-lattice sampling, and append predefined behavior vocabularies.
Simplify cmd_vel_control: pass reverse commands through directly as straight reverse (vyaw=0), keep smoothing only in the timer acceleration limiter, and apply rotate-first / force-turn gating only for forward motion.
Tune low-speed linear/angular deadbands for smoother low-speed control response.
Update GO2 geometry/safety parameters and obstacle-map filtering/dilation parameters used by planning.

github-actions · 2026-04-29T08:30:42Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (253.8 files/s, 37212.9 lines/s)

Language	files	blank	comment	code
Python	13	611	263	3382
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	931	360	4867

github-actions · 2026-04-29T08:33:25Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (248.4 files/s, 36501.9 lines/s)

Language	files	blank	comment	code
Python	13	611	265	3394
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	931	362	4879

github-actions · 2026-04-29T08:36:41Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (245.3 files/s, 36040.9 lines/s)

Language	files	blank	comment	code
Python	13	611	265	3394
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	931	362	4879

github-actions · 2026-04-29T08:40:21Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.13 s (331.1 files/s, 48654.9 lines/s)

Language	files	blank	comment	code
Python	13	611	265	3393
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	931	362	4878

target_pose is never cleared on arrival, so planning_node kept generating paths and the robot never stopped. Now publishes an empty path once within 0.3m of target_pose so cmd_vel_control stale-path protection kicks in. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

…d check target_pose is a camera-frame position (POI recorded as camera pose), but previously compared against robot control center which is 0.35m behind the camera, so dist_to_goal was always ~0.35m > 0.3m threshold. Now compare T[:3,3] (camera position) directly to target_pose. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

map_node already detects arrival (dist < 0.5m) and publishes poi_change, which clears target_pose in planning_node, which then publishes an empty path to stop the robot. The dist_to_goal guard here is unreachable. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Removed redundant path publishing when target_pose is None or all trajectories are in collision.

Keep planner forward-only and delegate reverse handling to cmd_vel, while adding front-obstacle reflex and simple rotate-first gating to reduce risky turn-and-go behavior. Made-with: Cursor

Drop front_dist publisher coupling and keep reverse selection fully inside planning cost with single reverse candidate and front-clearance threshold gating. Made-with: Cursor

Normalize EOF newlines for planning_node.py and cmd_vel_control.py. Made-with: Cursor

Clean up unresolved conflict marker that caused ruff syntax failure in CI. Made-with: Cursor

…amline velocity sampling Added a new function to generate predefined trajectory vocabularies, allowing for future behaviors to be easily appended. Simplified the velocity sampling in the trajectory generation by removing the reverse sample and directly using a linear space for forward velocities. Updated trajectory generation in the PlanningNode to incorporate the new vocabulary, enhancing maneuvering capabilities.

github-actions · 2026-04-29T14:59:52Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (253.6 files/s, 37677.0 lines/s)

Language	files	blank	comment	code
Python	13	619	274	3444
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	939	371	4929

…fy comments Updated the cost function in PlanningNode to improve clarity and maintainability. Introduced clearer comments regarding predefined backward trajectory penalties and streamlined the logic for determining when to apply reverse penalties. This refactor aims to enhance the understanding of trajectory evaluations and improve the overall planning logic.

github-actions · 2026-04-29T15:21:44Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.16 s (256.9 files/s, 38144.7 lines/s)

Language	files	blank	comment	code
Python	13	619	273	3443
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	939	370	4928

…bularies

github-actions · 2026-04-29T15:46:37Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.13 s (319.0 files/s, 47378.8 lines/s)

Language	files	blank	comment	code
Python	13	619	273	3443
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	30	29	191
JSON	2	8	0	58
TOML	1	4	0	57
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	939	370	4928

github-actions · 2026-05-09T00:50:32Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (248.2 files/s, 37037.0 lines/s)

Language	files	blank	comment	code
Python	13	621	273	3458
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	942	371	4955

github-actions · 2026-05-09T00:52:09Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (243.6 files/s, 36359.8 lines/s)

Language	files	blank	comment	code
Python	13	621	273	3458
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	942	371	4955

github-actions · 2026-05-09T00:55:08Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.20 s (209.4 files/s, 31380.0 lines/s)

Language	files	blank	comment	code
Python	13	625	279	3474
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	946	377	4971

…/planning

github-actions · 2026-05-09T05:50:14Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (248.4 files/s, 37593.1 lines/s)

Language	files	blank	comment	code
Python	13	630	285	3526
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	951	383	5023

go2 config update

github-actions · 2026-05-09T05:51:30Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (251.3 files/s, 38036.5 lines/s)

Language	files	blank	comment	code
Python	13	630	285	3526
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	951	383	5023

github-actions · 2026-05-11T06:10:14Z

cloc	github.com/AlDanial/cloc v 2.04 T=0.17 s (249.4 files/s, 37745.5 lines/s)

Language	files	blank	comment	code
Python	13	630	287	3524
Markdown	4	155	6	426
C++	4	60	38	409
Bourne Shell	15	41	9	272
Dockerfile	1	31	30	202
JSON	2	8	0	58
TOML	1	4	0	58
make	1	18	15	55
CMake	1	4	0	17
--------	--------	--------	--------	--------
SUM:	42	951	385	5021

xiaolefang-dm force-pushed the xiaole/optimized/planning branch from 7baa340 to ff67ae9 Compare April 29, 2026 08:30

xiaolefang and others added 11 commits April 29, 2026 22:11

fix(nav): stop robot properly when no active nav target

1dc69b9

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

change dist_to_goal

3f47431

Remove unnecessary path publishing in planning_node

70933de

Removed redundant path publishing when target_pose is None or all trajectories are in collision.

refine planning and cmd_vel coordination for safer maneuvering

31237ac

Keep planner forward-only and delegate reverse handling to cmd_vel, while adding front-obstacle reflex and simple rotate-first gating to reduce risky turn-and-go behavior. Made-with: Cursor

fix(planning): remove front_dist topic and gate reverse in planner cost

7dd4372

Drop front_dist publisher coupling and keep reverse selection fully inside planning cost with single reverse candidate and front-clearance threshold gating. Made-with: Cursor

chore: add trailing newlines to planning and control files

3a57d2c

Normalize EOF newlines for planning_node.py and cmd_vel_control.py. Made-with: Cursor

fix(control): remove leftover merge conflict marker in cmd_vel_control

5108b9c

Clean up unresolved conflict marker that caused ruff syntax failure in CI. Made-with: Cursor

xiaolefang-dm force-pushed the xiaole/optimized/planning branch from 59f7d8f to a7a9e12 Compare April 29, 2026 14:59

fix(planning): correct typo prefined -> predefined in trajectory voca…

a902327

…bularies

xiaolefang-dm requested a review from dvorak0 April 29, 2026 15:49

xiaolefang-dm force-pushed the xiaole/optimized/planning branch from 959795c to a902327 Compare May 9, 2026 00:51

test(control): pass reverse commands through directly

c6c6d4b

xiaolefang-dm added 2 commits May 9, 2026 13:19

test(control): simplify cmd vel smoothing and force large heading turns

62f9e79

Merge branch 'xiaole/test/cmd-vel-heading-turn' into xiaole/optimized…

7ca1988

…/planning

go2 config

d721212

go2 config update

fix(control): remove yaw accel limit for cmd_vel

9181274

Conversation

xiaolefang-dm commented Apr 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented Apr 29, 2026

Uh oh!

github-actions Bot commented May 9, 2026

Uh oh!

github-actions Bot commented May 9, 2026

Uh oh!

github-actions Bot commented May 9, 2026

Uh oh!

github-actions Bot commented May 9, 2026

Uh oh!

github-actions Bot commented May 9, 2026

Uh oh!

github-actions Bot commented May 11, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

1 participant

xiaolefang-dm commented Apr 29, 2026 •

edited

Loading